#include "ros/ros.h"
#include "serial/serial.h"
#include <string>
#include <sstream>
#include <iostream>
#include <fstream>
#include <vector>
#include <ctime>
#include <std_msgs/String.h>
#include <std_msgs/Empty.h>
#include <std_msgs/Float64.h>
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/TwistStamped.h>
#include <geometry_msgs/Vector3.h>
#include <geometry_msgs/Point.h>
#include <geometry_msgs/Pose.h>
#include <geometry_msgs/PoseWithCovarianceStamped.h>
#include <ros/package.h>


#define  BYTE0(dwTemp)       ( *( (uint8_t *)(&dwTemp)    )  )
#define  BYTE1(dwTemp)       ( *( (uint8_t *)(&dwTemp) + 1) )
#define  BYTE2(dwTemp)       ( *( (uint8_t *)(&dwTemp) + 2) )
#define  BYTE3(dwTemp)       ( *( (uint8_t *)(&dwTemp) + 3) )

typedef enum {
    UP_VERSION = 0x00,
    UP_STATUS = 0x01,
    UP_SENSER = 0x02,
    UP_RCDATA = 0x03,
    UP_GPSDATA = 0x04,
    UP_POWER = 0x05,
    UP_MOTOR = 0x06,
    UP_SENSER2 = 0x07,
    UP_FLYMODE = 0x0A,
    UP_SPEED = 0x0B,
    UP_PID1 = 0x10,
    UP_PID2 = 0x11,
    UP_PID3 = 0x12,
    UP_PID4 = 0x13,
    UP_PID5 = 0x14,
    UP_PID6 = 0x15,
    UP_RADIO = 0x40,
    UP_MSG = 0xEE,
    UP_CHECK = 0xEF,

    UP_REMOTER = 0x50,
    UP_PRINTF = 0x51,

    UP_USER_DATA1 = 0xF1,
    UP_USER_DATA2 = 0xF2,
    UP_USER_DATA3 = 0xF3,
    UP_USER_DATA4 = 0xF4,
    UP_USER_DATA5 = 0xF5,
    UP_USER_DATA6 = 0xF6,
    UP_USER_DATA7 = 0xF7,
    UP_USER_DATA8 = 0xF8,
    UP_USER_DATA9 = 0xF9,
    UP_USER_DATA10 = 0xFA,
} upmsgID_e;


#define  D_COMMAND_ACC_CALIB        0x01
#define  D_COMMAND_GYRO_CALIB        0x02
#define  D_COMMAND_MAG_CALIB        0x04
#define  D_COMMAND_BARO_CALIB        0x05
#define  D_COMMAND_ACC_CALIB_EXIT    0x20
#define  D_COMMAND_ACC_CALIB_STEP1    0x21
#define  D_COMMAND_ACC_CALIB_STEP2    0x22
#define  D_COMMAND_ACC_CALIB_STEP3    0x23
#define  D_COMMAND_ACC_CALIB_STEP4    0x24
#define  D_COMMAND_ACC_CALIB_STEP5    0x25
#define  D_COMMAND_ACC_CALIB_STEP6    0x26
#define  D_COMMAND_FLIGHT_LOCK        0xA0
#define  D_COMMAND_FLIGHT_ULOCK        0xA1

#define  D_ACK_READ_PID                0x01
#define  D_ACK_READ_VERSION            0xA0
#define  D_ACK_RESET_PARAM            0xA1

typedef enum {
    DOWN_COMMAND = 0x01,
    DOWN_ACK = 0x02,
    DOWN_RCDATA = 0x03,
    DOWN_POWER = 0x05,
    DOWN_FLYMODE = 0x0A,
    DOWN_PID1 = 0x10,//rpy angle rate
    DOWN_PID2 = 0x11,//rpy angle
    DOWN_PID3 = 0x12,//vx vy vz
    DOWN_PID4 = 0x13,//x y z
    DOWN_PID5 = 0x14,
    DOWN_PID6 = 0x15,
    DOWN_RADIO = 0x40,

    DOWN_REMOTER = 0x50,
    DOWN_POSITION = 0x60, //add by ljy
} downmsgID_e;
#define  POS_NEXT 1
#define  POS_NOW 0

#define  REFRESH_RATE       30
#define  START_WAIT_TIME    1.0f //second
#define  FLY_UP_TIME      4.0f //second
#define  START_WAIT_COUNT   (START_WAIT_TIME * REFRESH_RATE)
#define  FLY_UP_COUNT   (FLY_UP_TIME * REFRESH_RATE)

using namespace std;

typedef struct {
    float roll;
    float pitch;
    float yaw;
    float thrust;
    float trimPitch;
    float trimRoll;
    uint8_t ctrlMode;
    bool flightMode;
    bool RCLock;
} remoterData_t;

typedef struct {
    float x;
    float y;
    float z;
} pos_t;

typedef struct {
    pos_t currentPos;
    pos_t desiredPos;
} pos_now_next_t;

typedef struct {
    int16_t kp;
    int16_t ki;
    int16_t kd;
} PID_t;


serial::Serial ros_ser;
std::vector<pos_t> csvdata;
int csv_ind = 0;


void sendBySerial(uint8_t msgID, uint8_t data_0, uint8_t *pStruct, uint8_t StructSize) {
    uint8_t cmd[40];
//    uint8_t msgID = 0x50;
    cmd[0] = 0xAA;
    cmd[1] = 0xAF;
    cmd[2] = msgID;
    cmd[3] = StructSize + 1;
    cmd[4] = data_0;
    for (int i = 0; i < StructSize; i++)
    {
        cmd[5 + i] = pStruct[i];
    }
    uint8_t sum = 0;
    for (int i = 0; i < StructSize + 5; i++)
    {
        sum += cmd[i];
    }
    cmd[StructSize + 5] = sum;
    ros_ser.write(cmd, StructSize + 6);
    usleep(1000);
//    for (int i = 0; i < StructSize + 6; i++) {
//        cout << hex << int(cmd[i]) << "  ";
//    }
//    cout << dec << endl;
}


void sendBySerial(uint8_t msgID, uint8_t *pStruct, uint8_t StructSize) {
    uint8_t cmd[40];
//    uint8_t msgID = 0x50;
    cmd[0] = 0xAA;
    cmd[1] = 0xAF;
    cmd[2] = msgID;
    cmd[3] = StructSize;
    for (int i = 0; i < StructSize; i++)
    {
        cmd[4 + i] = pStruct[i];
    }
    uint8_t sum = 0;
    for (int i = 0; i < StructSize + 4; i++)
    {
        sum += cmd[i];
    }
    cmd[StructSize + 4] = sum;
    ros_ser.write(cmd, StructSize + 5);
    usleep(1000);

    for (int i = 0; i < StructSize + 5; i++)
    {
        cout << hex << int(cmd[i]) << "  ";
    }
    cout << dec << endl;
    cout << int(cmd[3]) << endl;

}

void read_serial() {
    std::string buffer;
    buffer = ros_ser.read(ros_ser.available());
    char msg_len = 0;
    char msg_type = 0;
    int i = 0;
    cout << "buffer:  " << buffer.size() << endl;
}

void initSendPID() {
    PID_t pidrr = {.kp=3000, .ki=0, .kd=65,};
    PID_t pidrp = {.kp=3000, .ki=0, .kd=65,};
    PID_t pidryaw = {.kp=2000, .ki=185, .kd=0,};
    PID_t pidr = {.kp=80, .ki=0, .kd=0,};
    PID_t pidp = {.kp=80, .ki=0, .kd=0,};
    PID_t pidyaw = {.kp=200, .ki=0, .kd=15,};

    PID_t pidvx = {.kp=45, .ki=0, .kd=0,};
    PID_t pidvy = {.kp=45, .ki=0, .kd=0,};
    PID_t pidvz = {.kp=1000, .ki=1500, .kd=100,};
    PID_t pidx = {.kp=40, .ki=0, .kd=6,};
    PID_t pidy = {.kp=40, .ki=0, .kd=6,};
    PID_t pidz = {.kp=60, .ki=0, .kd=45,};

    uint16_t thrustbase = 40000;
    int16_t trimR = 0;
    int16_t trimP = 0;


    pidrr.kp = 10000;
    pidrr.kd = 120;
    pidrp.kp = 10000;
    pidrp.kd = 120;
/*
    pidvz.kp = 2750;
    pidvz.kd = 550;
    pidvz.ki = 20;
    pidz.kp = 50;
    pidz.kd = 0;
    pidz.ki = 0;
*/

/*
    pidvz.kp = 1500;
    pidvz.kd = 550;
    pidvz.ki = 30;
    pidz.kp = 40;
    pidz.kd = 0;
    pidz.ki = 0;
*/

    pidvz.kp = 2750;
    pidvz.kd = 3000;
    pidvz.ki = 20;
    pidz.kp = 50;
    pidz.kd = 0;
    pidz.ki = 0;

//    pidvz.kp = 2750;
//    pidvz.kd = 50;
//    pidvz.ki = 20;
//    pidz.kp = 40;
//    pidz.kd = 30;
//    pidz.ki = 0;

//
    pidvx.kp = 60;
    pidvx.kd = 0;
    pidvy.kp = 60;
    pidvy.kd = 0;

    pidx.kp = 60;
    pidy.kp = 60;
    pidx.kd = 0;
    pidy.kd = 0;
    pidx.ki = 1;
    pidy.ki = 1;



    int pid_size = sizeof(PID_t);
    uint8_t temprate[pid_size * 3];
    uint8_t tempangle[pid_size * 3];
    uint8_t tempv[pid_size * 3];
    uint8_t temp[pid_size * 3];
    uint8_t tempthrutrim[sizeof(uint16_t) * 3];
    for (int i = 0; i < pid_size; i++)
    {
        temprate[i] = *((uint8_t *) (&pidrr) + i);
        temprate[pid_size + i] = *((uint8_t *) (&pidrp) + i);
        temprate[pid_size * 2 + i] = *((uint8_t *) (&pidryaw) + i);
        tempangle[i] = *((uint8_t *) (&pidr) + i);
        tempangle[pid_size + i] = *((uint8_t *) (&pidp) + i);
        tempangle[pid_size * 2 + i] = *((uint8_t *) (&pidyaw) + i);

        tempv[i] = *((uint8_t *) (&pidvx) + i);
        tempv[pid_size + i] = *((uint8_t *) (&pidvy) + i);
        tempv[pid_size * 2 + i] = *((uint8_t *) (&pidvz) + i);
        temp[i] = *((uint8_t *) (&pidx) + i);
        temp[pid_size + i] = *((uint8_t *) (&pidy) + i);
        temp[pid_size * 2 + i] = *((uint8_t *) (&pidz) + i);
    }
    tempthrutrim[0] = *((uint8_t *) (&thrustbase) + 0);
    tempthrutrim[1] = *((uint8_t *) (&thrustbase) + 1);
    tempthrutrim[2] = *((uint8_t *) (&trimR) + 0);
    tempthrutrim[3] = *((uint8_t *) (&trimR) + 1);
    tempthrutrim[4] = *((uint8_t *) (&trimP) + 0);
    tempthrutrim[5] = *((uint8_t *) (&trimP) + 1);
    sendBySerial(DOWN_PID1, temprate, sizeof(temprate));
    usleep(1000);
    sendBySerial(DOWN_PID2, tempangle, sizeof(tempangle));
    usleep(1000);
    sendBySerial(DOWN_PID3, tempv, sizeof(tempv));
    usleep(1000);
    sendBySerial(DOWN_PID4, temp, sizeof(temp));
    usleep(1000);
    sendBySerial(DOWN_PID5, tempthrutrim, sizeof(tempthrutrim));
    usleep(1000);
    ROS_INFO_STREAM("initSendPID end.");
}


pos_t init_pos;
bool cam_pos_inited = false;
bool vicon_pos_inited = false;

geometry_msgs::PoseStamped cam_origin,cam_reset_origin_pos;
geometry_msgs::PoseStamped vicon_origin,vicon_reset_origin_pos;
geometry_msgs::PoseStamped pos_setpoint_msg;
ros::Time start_timepoint;

void camposcallback(const geometry_msgs::PoseStamped::ConstPtr &msg) {
    pos_t current_pos_temp, current_pos;
    current_pos_temp.x = (float) msg->pose.position.x;
    current_pos_temp.y = -(float) msg->pose.position.y;
    current_pos_temp.z = (float) msg->pose.position.z;
    if (cam_pos_inited)
    {
        current_pos.x = current_pos_temp.x - init_pos.x;
        current_pos.y = current_pos_temp.y - init_pos.y;
        current_pos.z = current_pos_temp.z - init_pos.z;

        cam_reset_origin_pos = *msg;
        cam_reset_origin_pos.header.stamp = ros::Time::now();
        cam_reset_origin_pos.pose.position.x -= cam_origin.pose.position.x;
        cam_reset_origin_pos.pose.position.y -= cam_origin.pose.position.y;
        cam_reset_origin_pos.pose.position.z -= cam_origin.pose.position.z;
    } else
    {
        cam_origin = *msg;
        init_pos = current_pos_temp;
        current_pos.x = 0;
        current_pos.y = 0;
        current_pos.z = 0;
        cam_pos_inited = true;
    }


    printf("current_pos: %7.3f%7.3f%7.3f\n", current_pos.x, current_pos.y, current_pos.z);
//    cout << setprecision(3) << current_pos.x << " " << current_pos.y << " " << current_pos.z << endl;
    sendBySerial(DOWN_POSITION, POS_NOW, (uint8_t *) &current_pos, sizeof(current_pos));
//        sendBySerial(DOWN_POSITION,POS_NEXT,(uint8_t *) &desired_pos,sizeof(desired_pos));
    usleep(1000);
}

void viconposcallback(const geometry_msgs::PoseStamped::ConstPtr &msg) {

    if (vicon_pos_inited)
    {

        vicon_reset_origin_pos = *msg;
        vicon_reset_origin_pos.header.stamp = ros::Time::now();
        vicon_reset_origin_pos.pose.position.x -= vicon_origin.pose.position.x;
        vicon_reset_origin_pos.pose.position.y -= vicon_origin.pose.position.y;
        vicon_reset_origin_pos.pose.position.z -= vicon_origin.pose.position.z;
    } else {
        vicon_origin = *msg;
        vicon_pos_inited = true;
    }
}



void fusedposcallback(const geometry_msgs::PoseStamped::ConstPtr &msg) {
//    cout << "fused:  " << msg->pose.position.x << "\t" << msg->pose.position.y << "\t" << msg->pose.position.z << endl;
}

int main(int argc, char **argv) {
    // raise(SIGSTOP);
    ros::init(argc, argv, "send_serial_node");
    ros::NodeHandle nodeHandle;

    std::string dev_name = "/dev/ttyACM0";
    int baudrate = 500000;
    ros::Rate loop_rate(REFRESH_RATE);

    FILE *fp;
    std::string pkg_path = ros::package::getPath("drone_pnp");
    fp = fopen((pkg_path + "/data/8-traj.csv").c_str(), "r");
    if (fp)
    { std::cout << "open path csv" << std::endl; }
    while (1)
    {
        pos_t pointtemp;
        fscanf(fp, "%f,%f,%f", &pointtemp.x, &pointtemp.y, &pointtemp.z);
        csvdata.push_back(pointtemp);
        if (feof(fp))break;
    }
    fclose(fp);

    ofstream vicon_cam_data_file;
    vicon_cam_data_file.open(pkg_path+"/data/vicon_cam_data_file.csv");

    try
    {
        ros_ser.setPort(dev_name);
        // ros_ser.setPort("/dev/serial/by-id/usb-STMicroelectronics_STM32_Virtual_COM_Port_6D87155F5056-if00");
        ros_ser.setBaudrate(baudrate);
        serial::Timeout to = serial::Timeout::simpleTimeout(1000);
        ros_ser.setTimeout(to);
        ros_ser.open();
    }
    catch (serial::IOException &e)
    {
        ROS_ERROR_STREAM("Unable to open port ");
    }
    if (ros_ser.isOpen())
    {
        ROS_INFO_STREAM("Serial Port opened");
    } else
    {
        ROS_ERROR_STREAM("No port opened");
    }

    initSendPID();

//    while (ros::ok()); //todo

    ros::Subscriber cam_pos_sub = nodeHandle.subscribe<geometry_msgs::PoseStamped>("/cam/pose", 1, camposcallback);
    ros::Subscriber vicon_pos_sub = nodeHandle.subscribe<geometry_msgs::PoseStamped>("/mocap/pose", 1, viconposcallback);
//    ros::Subscriber drone_pos_sub = nodeHandle.subscribe<geometry_msgs::PoseStamped>("/fused_pos/pose", 1,
//                                                                                     fusedposcallback);
    ros::Publisher cam_reset_origin_pub = nodeHandle.advertise<geometry_msgs::PoseStamped> ("/cam_reset_origin/pose", 1);
    ros::Publisher vicon_reset_origin_pub = nodeHandle.advertise<geometry_msgs::PoseStamped> ("/vicon_reset_origin/pose", 1);
    ros::Publisher pos_setpoint_pub = nodeHandle.advertise<geometry_msgs::PoseStamped> ("/pos_setpoint/pose", 1);


    ros::spinOnce();

//    ros::spin();
    int j = 0, csv_ind = 0;
    start_timepoint = ros::Time::now();

    while (ros::ok())
    {
//        uint8_t cmd[]={0xAA,0xAF,0x50,0x11,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00};
//        uint8_t cksum = 0xCA;
//        cmd[2] = cmd[2];
//        int cnt = 5;
//        float tmp;
//        for(int i = 0; i<4; ++i){
//            tmp = rpyt[i];
//            cmd[cnt++] = BYTE0(tmp);
//            cmd[cnt++] = BYTE1(tmp);
//            cmd[cnt++] = BYTE2(tmp);
//            cmd[cnt++] = BYTE3(tmp);
//            cksum += BYTE3(tmp) + BYTE2(tmp) + BYTE1(tmp) + BYTE0(tmp);
//            // printf("%02hhX %02hhX %02hhX %02hhX ", BYTE0(tmp), BYTE1(tmp), BYTE2(tmp), BYTE3(tmp));
//        }
//        cmd[cnt] = cksum;
//        // printf("%02hhX\n",cksum);
//        ros_ser.write(cmd,22);
//        // cout<<"rpyt_cmd = "<<rpyt[0]<<" "<<rpyt[1]<<" "<<rpyt[2]<<" "<<rpyt[3]<<endl;


//        remoterData_t remoterData;
//        remoterData.roll=0;
//        remoterData.pitch=0;
//        remoterData.yaw=0;
//
//        remoterData.thrust=20.0f;BYTE3
//        remoterData.trimPitch=0;
//        remoterData.trimRoll=0;
////        remoterData.ctrlMode=0;
////        remoterData.flightMode=true;
////        remoterData.RCLock=true;
//        sendBySerial(DOWN_REMOTOR,1,(uint8_t *) &remoterData,sizeof(remoterData));
//        cout << sizeof(remoterData) << endl;
//        cout<<endl;

        if (j > START_WAIT_COUNT)
        {
            pos_t current_pos, desired_pos;
            current_pos.x = 0;
            current_pos.y = 0;
            current_pos.z = 0;
            desired_pos.x = 0;
            desired_pos.y = 0;
            desired_pos.z = j > START_WAIT_COUNT+FLY_UP_COUNT ? 0.6f : (j - START_WAIT_COUNT) / FLY_UP_COUNT * 0.6f;
            pos_now_next_t pos_now_next;
            pos_now_next.currentPos = current_pos;
            pos_now_next.desiredPos = desired_pos;

            if(j > START_WAIT_COUNT+FLY_UP_COUNT+ REFRESH_RATE*3 && csv_ind < csvdata.size())
            {
                desired_pos = csvdata[csv_ind++];
                desired_pos.x *= 0.5;
                desired_pos.y *= 1;
                desired_pos.z = .6;
            }

//            sendBySerial(DOWN_POSITION, POS_NOW, (uint8_t *) &current_pos, sizeof(current_pos));
//            usleep(1000);
            sendBySerial(DOWN_POSITION, POS_NEXT, (uint8_t *) &desired_pos, sizeof(desired_pos));
//            usleep(1000);
//            cout << desired_pos.x << "  " << desired_pos.y << "  " << desired_pos.z << endl;
            pos_setpoint_msg.header.stamp = ros::Time::now();
            pos_setpoint_msg.pose.position.x = desired_pos.x;
            pos_setpoint_msg.pose.position.y = -desired_pos.y;
            pos_setpoint_msg.pose.position.z = desired_pos.z;
        }
        j++;
//        if (j == 399)
//            j = 0;

        double cam_time = (cam_reset_origin_pos.header.stamp - start_timepoint).toSec();
        double vicon_time = (vicon_reset_origin_pos.header.stamp - start_timepoint).toSec();


        vicon_cam_data_file << vicon_time <<","
                << vicon_reset_origin_pos.pose.position.x << ","
                << vicon_reset_origin_pos.pose.position.y << ","
                << vicon_reset_origin_pos.pose.position.z << ","
                << cam_time <<","
                << cam_reset_origin_pos.pose.position.x << ","
                << cam_reset_origin_pos.pose.position.y << ","
                << cam_reset_origin_pos.pose.position.z << ","
                << pos_setpoint_msg.pose.position.x << ","
                << pos_setpoint_msg.pose.position.y << ","
                << pos_setpoint_msg.pose.position.z
                <<endl;
        vicon_reset_origin_pub.publish(vicon_reset_origin_pos);
        cam_reset_origin_pub.publish(cam_reset_origin_pos);
        pos_setpoint_pub.publish(pos_setpoint_msg);

        loop_rate.sleep();
        ros::spinOnce();
//        cout << endl << endl;
    }


    return 0;
}
